#include "femdemo.h"
#include <cmath>
#include <fstream>
#include <iomanip>

using std::setw ;
using std::setiosflags;
#define pi 4*atan(1.0)
#define inf 1e-9 
void Q1Element::set_weight(int _acc)
{
    string filename;
    string pathname = "GaussPntDoc/quadrilateral/";
    switch (_acc)
    {
    case 1: filename = "1.txt"; break;
    case 2:
    case 3: filename = "3.txt"; break;
    case 4:
    case 5: filename = "5.txt"; break;
    case 6:
    case 7: filename = "7.txt"; break;
    case 8:
    case 9: filename = "9.txt"; break;
    case 10:
    case 11: filename = "11.txt";break;    
    default: filename = "19.txt"; break;
    }
    filename = pathname + filename ;
    ifstream is(filename.c_str());
    {
        int n_point;
        is >> n_point;
        weight.resize(n_point);
        for(int i=0; i < n_point; i++)
        {
            point p;
            double _weight;
            is >> p[0];
            is >> p[1];
            is >> _weight;
            weight[i] = make_pair(p,_weight);
        }
    }
    is.close();
};
Q1Element:: Q1Element(int _acc )
{
    index = -1 ;
    set_weight(_acc);
}
double Q1Element::basic_function(int _i , double _xi, double _eta)
{
    double res = 0;
    switch(_i)
    {
        case 0  :   res = (_xi -1) *(_eta -1) /4;   break ;
        case 1  :   res = -(_xi +1) *(_eta -1) /4;  break;
        case 2  :   res = (_xi +1) *(_eta +1) /4;   break;
        case 3  :   res = -(_xi -1) *(_eta +1) /4;  break;
    }
    return res ;
};

pair<double,double> Q1Element:: grad_basic_function(int _i, double _xi, double _eta)
{
    double partial_xi = 0 ;
    double partial_eta = 0 ;
    switch(_i)
    {
        case 0  :   partial_xi = (_eta -1) /4   ;   partial_eta = (_xi -1) /4 ;     break;
        case 1  :   partial_xi = -(_eta -1) /4  ;   partial_eta = -(_xi +1) /4;     break;
        case 2  :   partial_xi = (_eta +1) /4   ;   partial_eta = (_xi +1) /4;      break;
        case 3  :   partial_xi = -(_eta +1) /4  ;   partial_eta = -(_xi -1) /4;     break;
    };
    pair<double,double> res {partial_xi, partial_eta};
    return res;
};

Matrix2d Q1Element:: get_Jacobian(point _p) 
{
    Matrix2d res = Matrix2d::Zero(2,2);
    double xi = _p[0];
    double eta = _p[1];
    const vector<point> vet = this->get_vertexes();
    for(int i =0; i < 4; i++)
    {
        res(0,0) += vet[i][0]* grad_basic_function(i,xi,eta).first;
        res(0,1) += vet[i][1]* grad_basic_function(i,xi,eta).first;
        res(1,0) += vet[i][0]* grad_basic_function(i,xi,eta).second;
        res(1,1) += vet[i][1]* grad_basic_function(i,xi,eta).second;
    }
    return res;
};

Matrix2d Q1Element:: get_inv_Jacobian(point _p)
{
    Matrix2d jacobian = this-> get_Jacobian(_p);
    double det = jacobian(0,0)*jacobian(1,1) - jacobian(0,1)*jacobian(1,0);
    Matrix2d res;
    res(0,0) = jacobian(1,1);
    res(0,1) = -jacobian(0,1);
    res(1,0) = -jacobian(1,0);
    res(1,1) = jacobian(1,1) ;
    res /= det;
    return res;
};

double Q1Element:: con_Matrix_a_ij(int _i, int _j)
{
    const vector<pair<point,double> >  _wei = this->get_weight();
    int n_wei_point = _wei.size() ;
    double res = 0 ;
    for(int i =0; i < n_wei_point; i++)
    {
        Matrix2d jacobian = this-> get_Jacobian(_wei[i].first);
        double det = jacobian(0,0)*jacobian(1,1) - jacobian(0,1)*jacobian(1,0);
        Matrix2d inv_jacobian;
        inv_jacobian(0,0) = jacobian(1,1);
        inv_jacobian(0,1) = -jacobian(0,1);
        inv_jacobian(1,0) = -jacobian(1,0);
        inv_jacobian(1,1) = jacobian(1,1) ;
        inv_jacobian /= det;
        pair<double,double> grad_i = grad_basic_function(_i, _wei[i].first[0], _wei[i].first[1]);
        pair<double,double> grad_j = grad_basic_function(_j, _wei[i].first[0], _wei[i].first[1]);
        double par_fai_i_par_x = grad_i.first * inv_jacobian(0,0) + grad_i.second * inv_jacobian(0,1);
        double par_fai_j_par_x = grad_j.first * inv_jacobian(0,0) + grad_j.second * inv_jacobian(0,1);
        double par_fai_i_par_y= grad_i.first * inv_jacobian(1,0) + grad_i.second * inv_jacobian(1,1);
        double par_fai_j_par_y= grad_j.first * inv_jacobian(1,0) + grad_j.second * inv_jacobian(1,1);
        res += _wei[i].second * det *(par_fai_i_par_x * par_fai_j_par_x + par_fai_i_par_y *par_fai_j_par_y);
    }
    return res ;
};
pair<pair<size_t,size_t>,double> Q1_boundary_element::con_Matrix_aij(int _i, int _j)
{
    int ii = basic_index[_i];
    int jj = basic_index[_j];
    const vector<pair<point,double> >  _wei = this->get_weight();
    int n_wei_point = _wei.size() ;
    double res = 0 ;
    for(int i =0; i < n_wei_point; i++)
    {
        Matrix2d jacobian = this-> get_Jacobian(_wei[i].first);
        double det = jacobian(0,0)*jacobian(1,1) - jacobian(0,1)*jacobian(1,0);
        Matrix2d inv_jacobian;
        inv_jacobian(0,0) = jacobian(1,1);
        inv_jacobian(0,1) = -jacobian(0,1);
        inv_jacobian(1,0) = -jacobian(1,0);
        inv_jacobian(1,1) = jacobian(1,1) ;
        inv_jacobian /= det;
        pair<double,double> grad_i = grad_basic_function(ii, _wei[i].first[0], _wei[i].first[1]);
        pair<double,double> grad_j = grad_basic_function(jj, _wei[i].first[0], _wei[i].first[1]);
        double par_fai_i_par_x = grad_i.first * inv_jacobian(0,0) + grad_i.second * inv_jacobian(0,1);
        double par_fai_j_par_x = grad_j.first * inv_jacobian(0,0) + grad_j.second * inv_jacobian(0,1);
        double par_fai_i_par_y= grad_i.first * inv_jacobian(1,0) + grad_i.second * inv_jacobian(1,1);
        double par_fai_j_par_y= grad_j.first * inv_jacobian(1,0) + grad_j.second * inv_jacobian(1,1);
        res += _wei[i].second * det *(par_fai_i_par_x * par_fai_j_par_x + par_fai_i_par_y *par_fai_j_par_y);
    }
    pair<size_t,size_t> p{dofs[_i],dofs[_j]};
    return make_pair(p,res);
    
}
void RegularMesh:: build_regularmesh(int _acc, point _pleft, point _pright, size_t _nx, size_t _ny)
{
    pleft = _pleft;
    pright = _pright;
    nx = _nx;
    ny = _ny;
    double lenx = pright[0] - pleft[0];
    double leny = pright[1] - pleft[1];
    double hx = lenx / nx;
    double hy = leny / ny ;
    int num_ele = (nx -2)*(ny-2);
    //以下都是对内部单元内的设置
    elements.resize(num_ele);
    dofs.resize((nx-1)*(ny-1));
    //设置自由度的坐标
    size_t n_dofs = 0 ;
    for(int j =0; j < ny -1; j++)
        for(int i =0; i < nx-1; i++)
        {
            point p;
            p[0] = pleft[0]+ hx + i * hx ;
            p[1] = pleft[1] + hy + j * hy ;
            dofs[n_dofs] = p ;
            n_dofs ++;
        }
    for(int i = 0; i < ny-2; i++)
        for(int j =0; j < nx-2; j++)
        {
            Q1Element ele(_acc);
            //设置序号
            ele.set_index(i*(nx-2)+j);
            //设置自由度编号
            vector<size_t> dof(4);
            dof[0] = j + i*(nx-1);
            dof[1] = j + 1 +i *(nx -1);
            dof[2] = j + 1 +(i+1) *(nx-1);
            dof[3] = j + (i+1) *(nx-1);
            ele.set_dofs(dof);
            //设置顶点坐标
            vector<point> vet(4);
            vet[0][0] = pleft[0] + hx + j * hx ;
            vet[0][1] = pleft[1] + hy + i * hy ;
            vet[1][0] = pleft[0] + hx + (j+1) * hx ;
            vet[1][1] = pleft[1] + hy + i * hy ;
            vet[2][0] = pleft[0] + hx + (j+1) * hx ;
            vet[2][1] = pleft[1] + hy + (i+1) * hy ;
            vet[3][0] = pleft[0] + hx + j * hx ;
            vet[3][1] = pleft[1] + hy + (i+1) * hy ;
            ele.set_vertexes(vet);

            elements[j+i*(nx-2)] = ele ;
        }
    //以下对边界的element进行设置
    //左下角第一个单元
    boundary_elements.resize(2*(nx+ny)-4);
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(1);
        dof[0] = 0 ;
        vector<size_t> _index(1);
        _index[0] = 2;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pleft[0] ;
        vet[0][1] = pleft[1] ;
        vet[1][0] = pleft[0] + hx ;
        vet[1][1] = pleft[1] ;
        vet[2][0] = pleft[0] + hx ;
        vet[2][1] = pleft[1] + hy ;
        vet[3][0] = pleft[0] ;
        vet[3][1] = pleft[1] + hy ;
        q1_b.set_vertexes(vet);
        boundary_elements[0] = q1_b;
    }
    //最下面一行单元
    for(int i =1; i < nx-1; i++)
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(2);
        dof[0] = i ;
        dof[1] = i-1;
        vector<size_t> _index(2);
        _index[0] = 2;
        _index[1] = 3;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pleft[0] + i *hx ;
        vet[0][1] = pleft[1] ;
        vet[1][0] = pleft[0] + hx + i *hx ;
        vet[1][1] = pleft[1] ;
        vet[2][0] = pleft[0] + hx + i *hx;
        vet[2][1] = pleft[1] + hy ;
        vet[3][0] = pleft[0] + i *hx;
        vet[3][1] = pleft[1] + hy ;
        q1_b.set_vertexes(vet);
        boundary_elements[i] = q1_b;
    }
    //右下角单元
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(1);
        dof[0] = nx - 2 ;
        vector<size_t> _index(1);
        _index[0] = 3;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pleft[0] + (nx-1) *hx ;
        vet[0][1] = pleft[1] ;
        vet[1][0] = pleft[0] + nx *hx ;
        vet[1][1] = pleft[1] ;
        vet[2][0] = pleft[0] + nx *hx;
        vet[2][1] = pleft[1] + hy ;
        vet[3][0] = pleft[0] + (nx -1) *hx;
        vet[3][1] = pleft[1] + hy ;
        q1_b.set_vertexes(vet);
        boundary_elements[nx-1] = q1_b;
    }
    //最右边单元
    for(int j = 1; j < ny-1; j++)
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(2);
        dof[0] = (j+1) *(nx-1) -1 ;
        dof[1] = j *(nx-1) -1 ;
        vector<size_t> _index(2);
        _index[0] = 3;
        _index[1] = 0;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pleft[0] + (nx-1) *hx ;
        vet[0][1] = pleft[1] +j*hy;
        vet[1][0] = pleft[0] + nx *hx ;
        vet[1][1] = pleft[1] +j*hy;
        vet[2][0] = pleft[0] + nx *hx;
        vet[2][1] = pleft[1] + (j+1)*hy ;
        vet[3][0] = pleft[0] + (nx -1) *hx;
        vet[3][1] = pleft[1] + (j+1)*hy ;
        q1_b.set_vertexes(vet);
        boundary_elements[nx-1+j] = q1_b;
    }
    //右上角单元
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(1);
        dof[0] = (ny-1) *(nx-1) - 1 ;
        vector<size_t> _index(1);
        _index[0] = 0;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pleft[0] + (nx-1) *hx ;
        vet[0][1] = pleft[1] + (ny-1)*hy;
        vet[1][0] = pleft[0] + nx *hx ;
        vet[1][1] = pleft[1] +(ny-1)*hy;
        vet[2][0] = pleft[0] + nx *hx;
        vet[2][1] = pleft[1] + ny*hy ;
        vet[3][0] = pleft[0] + (nx -1) *hx;
        vet[3][1] = pleft[1] + ny*hy ;
        q1_b.set_vertexes(vet);
        boundary_elements[nx+ny-2] = q1_b;
    }
    //最上面单元
    for(int j = 1; j < nx-1; j++)
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(2);
        dof[0] = (ny-1) *(nx-1)-j -1 ;
        dof[1] = (ny -1)*(nx-1)-j ;
        vector<size_t> _index(2);
        _index[0] = 0;
        _index[1] = 1;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pright[0] - (j+1)* hx ;
        vet[0][1] = pright[1] - hy;
        vet[1][0] = pright[0] - j *hx ;
        vet[1][1] = pright[1] - hy;
        vet[2][0] = pright[0] - j *hx;
        vet[2][1] = pright[1] ;
        vet[3][0] = pright[0] - (j+1) *hx;
        vet[3][1] = pright[1] ;
        q1_b.set_vertexes(vet);
        boundary_elements[nx+ny-2+j] = q1_b;
    }
    //左上角单元
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(1);
        dof[0] = (nx -1)*(ny-2) ;
        vector<size_t> _index(1);
        _index[0] = 1;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pleft[0] ;
        vet[0][1] = pright[1] - hy;
        vet[1][0] = pleft[0] + hx ;
        vet[1][1] = pright[1] - hy;
        vet[2][0] = pleft[0] + hx;
        vet[2][1] = pright[1] ;
        vet[3][0] = pleft[0];
        vet[3][1] = pright[1] ;
        q1_b.set_vertexes(vet);
        boundary_elements[2*nx+ny-3] = q1_b;
    }
    //左边单元
    for(int j = 1; j < ny-1; j++)
    {
        Q1_boundary_element q1_b(_acc);
        q1_b.set_index(num_ele ++);
        vector<size_t> dof(2);
        dof[0] = (ny- 2 -j) *(nx-1) ;
        dof[1] = (ny -1 -j) *(nx-1) ;
        vector<size_t> _index(2);
        _index[0] = 1;
        _index[1] = 2;
        q1_b.set_basicindex(_index);
        q1_b.set_dofs(dof);
        vector<point> vet(4);
        vet[0][0] = pleft[0] ;
        vet[0][1] = pright[1] -(j+1)* hy;
        vet[1][0] = pleft[0] + hx ;
        vet[1][1] = pright[1] - (j + 1) * hy;
        vet[2][0] = pleft[0] + hx ;
        vet[2][1] = pright[1] - j *hy;
        vet[3][0] = pleft[0];
        vet[3][1] = pright[1] - j* hy ;
        q1_b.set_vertexes(vet);
        boundary_elements[2*nx+ny-3+j] = q1_b;
    }
    
};

void RegularMesh::fem_solution( string str,double (*f)(double,double))
{
    size_t n_ele = (nx -2) * (ny -2) ;
    size_t n_dofs = (nx-1) *(ny -1) ;
    MatrixXd A = MatrixXd::Zero(n_dofs,n_dofs);
    vector<pair<point, double> > _wei = elements[0].get_weight();
    size_t n_bound = boundary_elements.size();
    VectorXd F = VectorXd::Zero (n_dofs);
    int wei_size = _wei.size();
    double hx = (pright[0] - pleft[0]) / nx;
    double hy = (pright[1] - pleft[1]) / ny;
    for(size_t i =0; i < n_ele; i++)
    {
        const vector<size_t> dofs = elements[i].get_dofs();
        for(int j =0; j < 4; j++)
            for(int k =0; k < 4; k++)
            {
                A(dofs[j],dofs[k]) += elements[i].con_Matrix_a_ij(j,k);
            }
        const vector<point> vet = elements[i].get_vertexes();
        double x , y ;// 将参考单元积分点坐标转成全局坐标
        for(int j =0; j < wei_size; j++)
        {
            for(int k =0 ; k < 4; k++)
            {
                Matrix2d jacobi = elements[i].get_Jacobian(_wei[j].first);
                double det = jacobi(0,0)*jacobi(1,1) - jacobi(1,0)*jacobi(0,1) ;
                x = (_wei[j].first[0] + 1 ) / 2 * hx + vet[0][0] ;
                y = (_wei[j].first[1] + 1 ) / 2 * hy + vet[0][1] ;
                F(dofs[k]) += _wei[j].second * elements[i].basic_function(k, _wei[j].first[0], _wei[j].first[1])
                                *det *((*f)(x,y));
            }                
        }
    }
    for(size_t i =0; i < n_bound; i++)
    {
        const vector<size_t> dofs = boundary_elements[i].get_dofs();
        const vector<size_t> _index = boundary_elements[i].get_basicindex();
        for(int j =0; j < dofs.size(); j++)
            for(int k =0; k < dofs.size(); k++)
            {
                pair<pair<size_t,size_t>,double> res = boundary_elements[i].con_Matrix_aij(j ,k);
                A(res.first.first,res.first.second) += res.second;
            }
        const vector<point> vet = boundary_elements[i].get_vertexes();
        double x , y ;// 将参考单元积分点坐标转成全局坐标
        for(int j =0; j < wei_size; j++)
        {
            for(int k =0 ; k < dofs.size(); k++)
            {
                Matrix2d jacobi = boundary_elements[i].get_Jacobian(_wei[j].first);
                double det = jacobi(0,0)*jacobi(1,1) - jacobi(1,0)*jacobi(0,1) ;
                x = (_wei[j].first[0] + 1 ) / 2 * hx + vet[0][0] ;
                y = (_wei[j].first[1] + 1 ) / 2 * hy + vet[0][1] ;
                int ki = _index[k];
                F(dofs[k]) += _wei[j].second * boundary_elements[i].basic_function(ki, _wei[j].first[0], _wei[j].first[1])
                                *det *((*f)(x,y));
            }                
        }
    }
    std::cout << F << std::endl << std::endl ;
    VectorXd X;
    X = A.colPivHouseholderQr().solve(F); //直接求解
     VectorXd Y = A * X ;
    ofstream os(str.c_str());
    const vector<point> p = get_dofs();
    os << "  nx:  " << nx << "   ny: " << ny << std::endl ;
    for(size_t i =0; i < nx + 1; i++)
    {
        os <<  setiosflags(std::ios::right);
        os << setw(15) << pleft[0]+ i * hx << setw(15) << pleft[1] <<setw(15) << 0.0 << std::endl ;
    }
    // for(size_t i = 0; i < ny -1 ; i++)
    // {
    //     os <<  setiosflags(std::ios::right);
    //     os << setw(15) << pleft[0] << setw(15) << pleft[1] + (i+1)*hy <<setw(15) << 0.0 << std::endl ;
    //     os <<  setiosflags(std::ios::right);
    //     os << setw(15) << pright[0] << setw(15) << pleft[1] + (i+1)*hy <<setw(15) << 0.0 << std::endl ;
    // }
    
    for(int i =0; i < p.size(); i++)
    {
        if(i % (nx-1) == 0)
        {
            os <<  setiosflags(std::ios::right);
            os << setw(15) << pleft[0] << setw(15) << pleft[1] + (i/(nx-1))*hy <<setw(15) << 0.0 << std::endl ;
        }
        os <<  setiosflags(std::ios::right);
        os << setw(15) << p[i][0] << setw(15) <<p[i][1]<<setw(15) << X(i) << std::endl ;
        if(i % (nx -1) == nx - 2)
        {
            os <<  setiosflags(std::ios::right);
            os << setw(15) << pright[0] << setw(15) << pleft[1] + (i/(nx-1))*hy <<setw(15) << 0.0 << std::endl ;
        }
    }
    for(size_t i =0; i < nx + 1; i++)
    {
        os <<  setiosflags(std::ios::right);
        os << setw(15) << pleft[0]+ i * hx << setw(15) << pright[1] <<setw(15) << 0.0 << std::endl ;
    }
    os.close();
    std::cout << Y-F << std::endl ;
    std::cout << "A" << std::endl;
    std::cout << A << std::endl;
    std::cout << "F" << std::endl;
    std::cout << F << std::endl;
}

double function(double x, double y)
{
    return 1;
}

int main(int argc, char *argv[])
{
    RegularMesh mesh;
    point pleft, pright ;
    int nx, ny, acc ;
    if(argc == 1){
        pleft[0] = -1;
        pleft[1] = -1;
        pright[0] = 1;
        pright[1] = 1;
        nx = 19;
        ny = 19 ;
        acc = 2 ;
    }
    else{
        pleft[0] = atoi(argv[1]);
        pleft[1] = atoi(argv[2]);
        pright[0] = atoi(argv[3]);
        pright[1] = atoi(argv[4]);
        nx = atoi(argv[5]);
        ny = atoi(argv[6]);
        acc = atoi(argv[7]);
    }
    mesh.build_regularmesh(acc,pleft,pright,nx,ny);
    double (*f) (double,double) ;
    f = function;
    mesh.fem_solution("res_data.txt",f);
    return 0 ;
}

#undef pi 
#undef inf 

// g++ -o femdemo femdemo.cpp -std=c++11 -I /usr/include/eigen3/ -g

